#include "tio_gripper.h"
// #include "rs485msgque.h"
#include "rs485cmdque.h"
// #include "rtapi/rtapi.h"

static GripperStateUnit gripperState[MAX_TIO_SIG_NUM] = {};

GripperStateUnit* tio_gripper_list_signal() { return gripperState; }

GripperStateUnit* checkGripperSigByName(const char* name)
{
    for (int i = 0; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, name) == 0)
        {
            return &(gripperState[i]);
        }
    }
    return NULL;
}

int tio_gripper_has_signal()
{
    int count = 0;
    for (int i = 0; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, "") != 0)
        {
            count++;
        }
    }
    return count;
}

void tio_gripper_reset_send_state()
{
    for (int i = 0; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, "") != 0)
        {
            if (gripperState[i].commu_state != GRIPPER_WAITING_TO_SEND_ERROR && gripperState[i].commu_state != GRIPPER_WAITING_REC_ERROR)
            {
                gripperState[i].commu_state = GRIPPER_WAITING_TO_SEND_NORMAL;
            }
            else
            {
                gripperState[i].commu_state = GRIPPER_WAITING_TO_SEND_ERROR;
            }
            gripperState[i].count = 0;
        }
    }
}

// 根据信号量是否查询完成，其指令队列可插入，来插入指令，保证更新效率
void checkSignalUpdate(const int* chn_enable, const int* chn_mode)
{
    static int i = 0;
    for (; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, "") != 0 && chn_enable[gripperState[i].chnId] && chn_mode[gripperState[i].chnId] == 0)
        {
            if (RsCmdQueIsFull())
            {
                break;  // 下一次继续从这个信号量开始尝试添加
            }
            if (gripperState[i].commu_state == GRIPPER_WAITING_TO_SEND_NORMAL || gripperState[i].commu_state == GRIPPER_WAITING_TO_SEND_ERROR)  // 并非已添加
            {
                RSRTUCMD rsRtuCmd;
                rsRtuCmd.type = RTU_TYPE;
                rsRtuCmd.state = WAITING_SEND;
                strcpy(rsRtuCmd.sigName, gripperState[i].sigName);
                if (gripperState[i].count < 1)  // 存在则不添加了
                {
                    RsCmdEnqueue(rsRtuCmd);
                    gripperState[i].count++;
                }
            }
            // prepare the next cycle
            if (gripperState[i].commu_state == GRIPPER_WAITING_TO_SEND_NORMAL)  // 处于错误状态时，仍然查询，但不重复报错
            {
                gripperState[i].commu_state = GRIPPER_WAITING_REC_NORMAL;
            }
            else if (gripperState[i].commu_state == GRIPPER_WAITING_TO_SEND_ERROR)
            {
                gripperState[i].commu_state = GRIPPER_WAITING_REC_ERROR;
            }
        }
    }
    i = i % MAX_TIO_SIG_NUM;  // 循环进行
}

int tio_gripper_add_signal(const char* name, int chnId, int slaveId, int type, int addr)
{
    if (chnId < 0 || chnId > 1)
    {
        return -1;
    }
    int res, i = 0, bExist = 0;
    // rtapi_print("signal: %s, %d, %d, %d, %d\n", name, chnId, slaveId, type, addr);
    if (name == NULL || strlen(name) > 128 || name[0] == 0)
        return -1;

    for (i = 0; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, name) == 0)
        {
            bExist = 1;
            break;
        }
    }
    if (bExist == 0)
    {
        for (i = 0; i < MAX_TIO_SIG_NUM; i++)
        {
            if (strcmp(gripperState[i].sigName, "") == 0)
            {
                break;
            }
        }
    }

    if (i < MAX_TIO_SIG_NUM)
    {
        strcpy(gripperState[i].sigName, name);
        gripperState[i].chnId = chnId;
        gripperState[i].slaveId = slaveId;
        gripperState[i].type = type;
        gripperState[i].addr = addr;
        gripperState[i].freq = 0;
        gripperState[i].count = 0;
        gripperState[i].prog_state_when_error = 0;
        gripperState[i].commu_state = GRIPPER_WAITING_TO_SEND_NORMAL;
    }

    res = (i >= MAX_TIO_SIG_NUM) ? -1 : 0;

    // print current signal list
    for (i = 0; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, "") != 0)
        {
            // rtapi_print("signal %d: %s, %d, %d\n", i, gripperState[i].sigName, gripperState[i].type, gripperState[i].addr);
        }
    }

    return res;
}

int tio_gripper_set_signal_prog_stat(const char* name, int prog_stat)
{
    GripperStateUnit* gripper_ptr = checkGripperSigByName(name);
    if (gripper_ptr)
    {
        gripper_ptr->prog_state_when_error = prog_stat;
    }
    else
    {
        return -1;
    }
    return 0;
}

int tio_gripper_del_signal(const char* name)
{
    int res, i = 0;
    if (name == NULL || strlen(name) > 128)
        return -1;

    for (i = 0; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, name) == 0)
        {
            memset(&(gripperState[i]), 0, sizeof(GripperStateUnit));
            break;
        }
    }
    // rtapi_print("after delete: i = %d, max = %d\n", i, MAX_TIO_SIG_NUM);

    // return -1 if not found
    res = (i >= MAX_TIO_SIG_NUM) ? -1 : 0;

    // print current signal list
    for (i = 0; i < MAX_TIO_SIG_NUM; i++)
    {
        if (strcmp(gripperState[i].sigName, "") != 0)
        {
            // rtapi_print("signal %d: %s, %d, %d\n", i, gripperState[i].sigName, gripperState[i].type, gripperState[i].addr);
        }
    }

    return res;  // signal not found
}

int tio_gripper_clr_signal(int chnId)
{
    int i = 0;
    if (chnId == 0 || chnId == 1)
    {
        for (i = 0; i < MAX_TIO_SIG_NUM; i++)
        {
            if (strcmp(gripperState[i].sigName, "") != 0 && gripperState[i].chnId == chnId)
            {
                memset(&(gripperState[i]), 0, sizeof(GripperStateUnit));
                gripperState[i].chnId = -1;
            }
        }
        // rtapi_print("signal of channel: %d removed\n", chnId);
    }
    return 0;
}

int tio_gripper_update_signal(const char* name, float freq, const int* chn_mode)
{
    if (name == NULL || strlen(name) > 128)
        return -1;

    GripperStateUnit* gripper_sig = checkGripperSigByName(name);
    if (gripper_sig)
    {
        // rtapi_print("update signal: %s, with freq: %f\n", gripper_sig->sigName, gripper_sig->freq);
        if (freq <= 0.000001)
        {
            gripper_sig->freq = 0;
        }
        else
        {
            gripper_sig->freq = (freq > 20 ? 20 : freq);
        }
        if (!RsCmdQueIsFull() && chn_mode[gripper_sig->chnId] == 0)
        {
            RSRTUCMD rsRtuCmd;
            rsRtuCmd.type = RTU_TYPE;
            rsRtuCmd.state = WAITING_SEND;
            strcpy(rsRtuCmd.sigName, gripper_sig->sigName);
            RsCmdEnqueue(rsRtuCmd);
            gripper_sig->count++;                                   // 不管是否有指令，手动刷新都添加一条指令
            gripper_sig->commu_state = GRIPPER_WAITING_REC_NORMAL;  // 手动刷新时重复报错，自动刷新不重复报错
        }
        else
        {
            // rtapi_print("update request ignored since queue is full\n");
        }
        return 0;
    }
    return -1;  // signal not found
}

int tio_gripper_send(int chnId, unsigned char data[32], unsigned int len)
{
    int i;
    if (data == NULL || len > 127)
    {
        return -1;
    }

    if (RsCmdQueIsFull())
    {
        return -1;
    }

    RSRTUCMD rsRtuCmd;
    rsRtuCmd.type = INST_TYPE;
    rsRtuCmd.state = WAITING_SEND;
    rsRtuCmd.instCmd.chnId = chnId;
    rsRtuCmd.instCmd.cmdLen = len;
    // rtapi_print("data send: ");
    for (i = 0; i < len; i++)
    {
        rsRtuCmd.instCmd.cmdData[i] = data[i];
        // rtapi_print("%d ", data[i]);
    }
    // rtapi_print(", chnId = %d\n", chnId);
    RsCmdEnqueue(rsRtuCmd);
    return 0;
}
